import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformBroadcaster
from tf_transformations import quaternion_from_euler
from cpp_can.msg import CarState
from yesense_interface.msg import EulerOnly  # type: ignore
import math

class OdomPublisher(Node):
    def __init__(self):
        super().__init__('dynamic_odom_tf_broadcaster')
        
        # 1. 声明参数（含新增轮距、IMU噪声阈值等关键参数）
        self.declare_parameter('publish_frequency', 100.0)    # 里程计发布频率（Hz）
        self.declare_parameter('print_frequency', 1.0)      # 日志打印频率（Hz）
        self.declare_parameter('wheelbase_length', 0.785)   # 轴距（前后轮距离，m）
        self.declare_parameter('imu_yaw_offset', 0.0)       # IMU yaw角校准偏移（°）
        self.declare_parameter('yaw_noise_threshold', 0.0025)# IMU yaw噪声阈值（过滤抖动）
        
        # 2. 读取参数
        self.publish_freq = self.get_parameter('publish_frequency').value
        self.print_freq = self.get_parameter('print_frequency').value
        self.L = self.get_parameter('wheelbase_length').value  # 轴距
        self.imu_yaw_offset = self.get_parameter('imu_yaw_offset').value
        self.yaw_noise_thresh = self.get_parameter('yaw_noise_threshold').value
        
        # 3. 里程计核心状态（新增IMU历史时间/角度，用于计算实际角速度）
        self.x = 0.0               # x轴位置（m，odom坐标系）
        self.y = 0.0               # y轴位置（m，odom坐标系）
        self.yaw_rad = 0.0         # 当前IMU偏航角（rad，实际行驶方向）
        self.last_yaw_rad = 0.0    # 上一帧IMU偏航角（rad）
        self.vx = 0.0              # 前进线速度（m/s，后轮速转换）
        self.angular = 0.0         # 实际偏航角速度
        self.steering_angle_rad = 0.0  # 转向角（rad，来自CarState）
        
        # 4. 时间戳管理（避免高频调用计算误差）
        self.last_odom_time = self.get_clock().now()  # 里程计发布时间
        self.last_print_time = self.get_clock().now() # 日志打印时间
        self.last_imu_time = self.get_clock().now()   # IMU数据时间（计算角速度用）
        
        # 5. 通信接口初始化
        # 订阅车辆状态（后轮速、转向角）
        self.car_state_sub = self.create_subscription(
            CarState, 'car_state', self.car_state_callback, 100)
        # 订阅IMU欧拉角（核心：获取实时yaw角）
        self.imu_sub = self.create_subscription(
            EulerOnly, 'euler_only', self.imu_callback, 100)
        # 发布里程计消息
        self.odom_pub = self.create_publisher(Odometry, 'odom', 100)
        # 广播odom→base_footprint TF变换
        self.tf_broadcaster = TransformBroadcaster(self)
        
        # 6. 定时器（按设定频率发布里程计）
        self.odom_timer = self.create_timer(1.0/self.publish_freq, self.publish_odometry)
        
        # 7. 启动日志
        self.get_logger().info('=== 平地里程计节点启动 ===')
        self.get_logger().info(f'发布频率: {self.publish_freq} Hz | 打印频率: {self.print_freq} Hz')
        self.get_logger().info(f'轴距: {self.L}m | IMU偏移: {self.imu_yaw_offset}°')

    def car_state_callback(self, msg):
        """处理车辆状态：更新线速度和转向角（仅读取，不推导角速度）"""
        # 后轮速转换：mm/s → m/s（假设CarState.back_wheel_speed为后轮平均速）
        self.vx = msg.back_wheel_speed / 1000.0
        # 转向角读取（若msg为角度，需加math.radians()，此处假设已为rad）
        self.steering_angle_rad = msg.turn_angle #接收数据时已经转化为rad单位
        # 过滤极小转向角（避免机械抖动被误判为转弯）
        if abs(self.steering_angle_rad) < 0.001:
            self.steering_angle_rad = 0.0

    def imu_callback(self, msg):
        """处理IMU数据：校准yaw角 + 计算实际偏航角速度（核心优化点）"""
        current_imu_time = self.get_clock().now()
        # 计算IMU前后帧时间差（避免除以极小值）
        imu_dt = (current_imu_time - self.last_imu_time).nanoseconds / 1e9
        if imu_dt <= 0.0005:
            return
        self.last_imu_time = current_imu_time
        
        # 1. IMU yaw角校准与归一化（[-180°, 180°] → [-π, π] rad）
        raw_yaw_deg = msg.euler.yaw + self.imu_yaw_offset  # 加校准偏移
        raw_yaw_deg = (raw_yaw_deg + 180) % 360 - 180      # 角度归一化
        self.yaw_rad = math.radians(raw_yaw_deg)           # 转换为弧度
        
        # 2. 计算yaw角变化量（避免跨-π/π突变，如179°→-179°实际变化2°）
        delta_yaw = self.yaw_rad - self.last_yaw_rad
        delta_yaw = (delta_yaw + math.pi) % (2 * math.pi) - math.pi
        # 3. 计算实际角速度（过滤噪声，避免传感器抖动）
        if abs(delta_yaw) > self.yaw_noise_thresh:
            self.angular = delta_yaw / imu_dt  # 角速度 = 角度变化 / 时间差


    def publish_odometry(self):
        """发布里程计消息和TF变换(动态选择运动模型:转弯/直线）"""
        current_time = self.get_clock().now()
        # 计算里程计更新时间差（控制频率稳定性）
        dt = (current_time - self.last_odom_time).nanoseconds / 1e9
        self.last_odom_time = current_time  
        delta_x = 0.0
        delta_y = 0.0
        delta_x = self.vx * math.cos(self.yaw_rad) * dt
        delta_y = self.vx * math.sin(self.yaw_rad) * dt
        self.last_yaw_rad = self.yaw_rad  #更新历史yaw角
        
        # 更新位置（累积delta_x/delta_y）
        self.x += delta_x
        self.y += delta_y
        
        # --------------------------
        # 1. 发布TF变换（odom → base_footprint）
        # --------------------------
        tf_msg = TransformStamped()
        tf_msg.header.stamp = current_time.to_msg()
        tf_msg.header.frame_id = 'odom'          # 父坐标系（里程计坐标系）
        tf_msg.child_frame_id = 'base_footprint' # 子坐标系（小车底座坐标系）
        # 位置
        tf_msg.transform.translation.x = self.x
        tf_msg.transform.translation.y = self.y
        tf_msg.transform.translation.z = 0.0     # 平地运动，z轴固定为0
        # 姿态（仅yaw角有效，pitch/roll=0）
        quat = quaternion_from_euler(0.0, 0.0, self.yaw_rad)
        tf_msg.transform.rotation.x = quat[0]
        tf_msg.transform.rotation.y = quat[1]
        tf_msg.transform.rotation.z = quat[2]
        tf_msg.transform.rotation.w = quat[3]
        # 广播TF
        self.tf_broadcaster.sendTransform(tf_msg)
        
        # --------------------------
        # 2. 发布里程计消息（nav_msgs/Odometry）
        # --------------------------
        odom_msg = Odometry()
        # 头部信息
        odom_msg.header.stamp = current_time.to_msg()
        odom_msg.header.frame_id = 'odom'
        odom_msg.child_frame_id = 'base_footprint'
        # 位置与姿态
        odom_msg.pose.pose.position.x = self.x
        odom_msg.pose.pose.position.y = self.y
        odom_msg.pose.pose.position.z = 0.0
        odom_msg.pose.pose.orientation.x = quat[0]
        odom_msg.pose.pose.orientation.y = quat[1]
        odom_msg.pose.pose.orientation.z = quat[2]
        odom_msg.pose.pose.orientation.w = quat[3]
        # 线速度与角速度（直线时角速度来自IMU，转弯时来自阿克曼）
        odom_msg.twist.twist.linear.x = self.vx
        odom_msg.twist.twist.linear.y = 0.0     # 平地无侧向速度
        odom_msg.twist.twist.linear.z = 0.0
        odom_msg.twist.twist.angular.z = self.angular #没有这个值
        # 发布里程计
        self.odom_pub.publish(odom_msg)
        
        # --------------------------
        # 3. 按频率打印日志（调试用）
        # --------------------------
        print_dt = (current_time - self.last_print_time).nanoseconds / 1e9
        if print_dt >= 1.0/self.print_freq:
            self.get_logger().info(
                f"位置: x={self.x:.3f}m, y={self.y:.3f}m | "
                f"线速: vx={self.vx:.3f}m/s | "
                f"Yaw: {math.degrees(self.yaw_rad):.1f}° | "
                f"dt: {dt:.5f}"
            )
            self.last_print_time = current_time

def main(args=None):
    # ROS 2节点初始化与自旋
    rclpy.init(args=args)
    odom_node = OdomPublisher()
    try:
        rclpy.spin(odom_node)
    except KeyboardInterrupt:
        odom_node.get_logger().info('节点被手动终止')
    finally:
        # 资源释放
        odom_node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
